#!/usr/bin/env python3

import rospy
import struct
import paho.mqtt.client as mqtt
from std_msgs.msg import String

class MqttSubCmd(object):
    def __init__(self):
        self.mqtt_client = mqtt.Client()
        self.mqtt_client.on_connect = self.on_connect
        self.mqtt_client.on_message = self.on_message
        self.mqtt_client.connect("test.mosquitto.org", 1883, 60)
        self.mqtt_client.loop_start()

        self.cmd_publisher = rospy.Publisher('/cmd_dest', String, queue_size=10)

    def on_connect(self, client, userdata, flags, rc):
        rospy.logwarn("[sub_cmd]Connected to MQTT broker with result code %s", str(rc))
        client.subscribe("wpr_dest_cmd")

    # 从MQTT网络接收数据，并发布到ROS网络中
    def on_message(self, client, userdata, mqtt_msg):
        rospy.logwarn("Received message on topic %s:  %s", mqtt_msg.topic, mqtt_msg.payload)
        if mqtt_msg.topic == "wpr_dest_cmd":
            #values = struct.unpack('fff', mqtt_msg.payload)
            payload_str = mqtt_msg.payload.decode()
            ros_msg = String()
            ros_msg.data = payload_str
            rospy.loginfo("[sub_cmd] 接收到MQTT服务器 Command:  %s ", ros_msg.data)
            self.cmd_publisher.publish(ros_msg)

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('sub_cmd')
    mqtt_subscriber = MqttSubCmd()
    mqtt_subscriber.run()